
#ifndef __FEG_ROBOT_H_
#define __FEG_ROBOT_H_

#include "ros/ros.h"
#include <iostream>
#include <string.h>
#include <string> 
#include <iostream>
#include <math.h>
#include <stdlib.h>    
#include <unistd.h>      
#include <sys/types.h>
#include <sys/stat.h>
#include <serial/serial.h>
#include <fcntl.h>          
#include <stdbool.h>
#include <tf/transform_broadcaster.h>
#include <std_msgs/String.h>
#include <std_msgs/Float32.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
using namespace std;

//宏定义
#define SEND_DATA_CHECK   	1       	    //发送数据校验标志位
#define READ_DATA_CHECK   	0        	    //接收数据校验标志位
#define FRAME_HEADER1     	0XAA     	    //帧头1
#define FRAME_HEADER2     	0X55     	    //帧头2
#define FRAME_TAIL        	0XCC      	    //帧尾
#define RECEIVE_DATA_SIZE 	36        	    //下位机发送过来的数据的长度
#define SEND_DATA_SIZE    	13         		//ROS向下位机发送的数据的长度
#define PI 				  	3.14159265f 	//圆周率
#define WHEEL_DIAMETER    	0.165f	 		//轮毂电机轮子直径  米
#define WHEEL_TRACK       	0.486f	 		//左右轮子中心距离  米
#define ENCODER_RESOLUTION  4096     		//轮子转一圈，编码器变化数值
#define ENCODER_MAX  		65535     		//编码器最大值
#define LEFT_SPEED_K		0.95f			//左电机速度比例系数
#define RIGHT_SPEED_K		1.0f			//右电机速度比例系数


//协方差矩阵，用于里程计话题数据，用于robt_pose_ekf功能包
const double odom_pose_covariance[36]   = {1e-3,    0,    0,   0,   0,    0, 
										      0, 1e-3,    0,   0,   0,    0,
										      0,    0,  1e6,   0,   0,    0,
										      0,    0,    0, 1e6,   0,    0,
										      0,    0,    0,   0, 1e6,    0,
										      0,    0,    0,   0,   0,  1e3 };

const double odom_pose_covariance2[36]  = {1e-9,    0,    0,   0,   0,    0, 
										      0, 1e-3, 1e-9,   0,   0,    0,
										      0,    0,  1e6,   0,   0,    0,
										      0,    0,    0, 1e6,   0,    0,
										      0,    0,    0,   0, 1e6,    0,
										      0,    0,    0,   0,   0, 1e-9 };

const double odom_twist_covariance[36]  = {1e-3,    0,    0,   0,   0,    0, 
										      0, 1e-3,    0,   0,   0,    0,
										      0,    0,  1e6,   0,   0,    0,
										      0,    0,    0, 1e6,   0,    0,
										      0,    0,    0,   0, 1e6,    0,
										      0,    0,    0,   0,   0,  1e3 };
										      
const double odom_twist_covariance2[36] = {1e-9,    0,    0,   0,   0,    0, 
										      0, 1e-3, 1e-9,   0,   0,    0,
										      0,    0,  1e6,   0,   0,    0,
										      0,    0,    0, 1e6,   0,    0,
										      0,    0,    0,   0, 1e6,    0,
										      0,    0,    0,   0,   0, 1e-9} ;

const float ticks_per_meter=WHEEL_DIAMETER*PI/ENCODER_RESOLUTION;


//速度、位置数据结构体
typedef struct __Vel_Pos_Data_
{
	float X;
	float Y;
	float Z;
	float left_speed;
	float right_speed;
	int left_enc;
	int right_enc;
	int last_left_enc;
	int last_right_enc;
	int left_change;
	int right_change;
	float left_d;
	float right_d;
	float d_xy_avg;
	float d_th;
	float dx;
	float dy;
}Vel_Pos_Data;

//ROS向下位机发送数据的结构体
typedef struct _SEND_DATA_  
{
	    uint8_t tx[SEND_DATA_SIZE];
		float X_speed;	       
		float Y_speed;           
		float Z_speed;
		float left_speed;
		float right_speed;        
		unsigned char Frame_Tail; 
}SEND_DATA;

//下位机向ROS发送数据的结构体
typedef struct _RECEIVE_DATA_     
{
	    uint8_t rx[RECEIVE_DATA_SIZE];
	    uint8_t Flag_Stop;
		unsigned char Frame_Header;
		float X_speed;  
		float Y_speed;  
		float Z_speed;  
		float Power_Voltage;	
		unsigned char Frame_Tail;
}RECEIVE_DATA;

//机器人底盘类，使用构造函数初始化数据和发布话题等
class feg_robot
{
	public:
		feg_robot();  //构造函数
		~feg_robot(); //析构函数
		void Control();   //循环控制代码
		serial::Serial Stm32_Serial; //声明串口对象 
	private:
		ros::NodeHandle n;           //创建ROS节点句柄
		ros::Time _Now, _Last_Time;  //时间相关，用于积分求位移(里程)
		float Sampling_Time;         //采样时间，用于积分求位移(里程)

		ros::Subscriber Cmd_Vel_Sub; //初始化话题订阅者
		tf::TransformBroadcaster odom_broadcaster; //实例化TF变换发布
		
		void Cmd_Vel_Callback(const geometry_msgs::Twist &twist_aux);  //速度话题订阅回调函数，其它小车使用此函数声明            
		ros::Publisher odom_publisher,voltage_publisher; //初始化话题发布者
		void Publish_Odom();     //发布里程计话题
		void Publish_Voltage();  //发布电源电压话题
		void Publish_TF();  //发布TF

        //从串口(ttyUSB)读取运动底盘速度,电源电压数据
        bool Get_Sensor_Data();   
        unsigned char Check_Sum(unsigned char Count_Number,unsigned char mode); //CRC校验函数
		short Data_Trans(uint8_t Data_High,uint8_t Data_Low); //高低位数据转化读取 返回下位机原始数值 short类型

        string usart_port_name, robot_frame_id, gyro_frame_id, odom_frame_id, cmd_vel;  //定义相关变量
        int serial_baud_rate;      //串口通信波特率
        RECEIVE_DATA Receive_Data; //串口接收数据结构体
        SEND_DATA Send_Data;       //串口发送数据结构体

        Vel_Pos_Data Robot_Pos;    //机器人的位置
        Vel_Pos_Data Robot_Vel;    //机器人的速度
        float Power_voltage;       //电源电压
};
#endif
